#ifndef CAN_H_
#define CAN_H_
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>

#include <iostream>
#include <string>
#include <cmath>
#include <list>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "nav_msgs/Odometry.h"
// #include "ultralytics_ros/breakState.h"
#include "can_ros1_driver/CarState.h"

#include <queue>
#include <deque>

class canControl
{
public:
    // 0 初始化
    canControl();

    // 1 下发CAN消息 速度控制节点消息的回调函数
    void velocityCallback(const geometry_msgs::Twist::ConstPtr &msg);
    void velocityCallback_Autoware(const geometry_msgs::TwistStamped::ConstPtr &msg);
    // void yoloCallback(const ultralytics_ros::breakState::ConstPtr &msg);
    void feedCallback(const can_ros1_driver::CarState::ConstPtr &msg);
    // 1.1 向底盘发送控制指令
    void publishCmd(float linear_vel, float angular_vel);

    void velocityCallback_Autoware_Timer(const ros::TimerEvent& event);

    // 2 析构函数
    ~canControl();

private:
    ros::NodeHandle nh_;
    // 是否为Autoware速度
    bool Autoware;
    // 接收的速度话题
    std::string twistTopic;
    // 轴距
    double wheel_base;
    // 套接字参数
    struct ifreq ifr;
    struct sockaddr_can can_addr;
    int sockfd;
    int ret;
    float linear_x_;
    float angular_z_;

    // 发送的数据
    struct can_frame frameE2;
    struct can_frame frame469;

    int driving_mode_;
    int gear_;

    // 停障信息
    bool bBreak;
    std::list<bool> break_list;
    double rate;

    // 订阅速度控制
    ros::Subscriber sub_;
    ros::Subscriber sub_feed;
    ros::Subscriber sub_yolo;
    ros::Timer timer_autoware_;

    // std::queue<double> vel_data_;
    std::deque<double> vel_data_;
    unsigned int vel_count_ = 0, last_vel_count_ = 0;
    char no_vel_cmd_count_ = 0;
};

#endif // CAN_H_
